#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim/msg/pose.hpp"
using geometry_msgs::msg::Twist;
using turtlesim::msg::Pose;
using std::placeholders::_1;
class TurtleControl:public rclcpp::Node
{
public:
    TurtleControl(const std::string node_name):Node(node_name)
    {
        velocity_publisher_=this->create_publisher<Twist>("turtle1/cmd_vel",10);
        pose_subscription_=this->create_subscription<Pose>("turtle1/pose",10,
                        std::bind(&TurtleControl::on_pose_received_,this,_1));
    }

private:
    void on_pose_received_(const Pose::SharedPtr pose)
    {
        auto message=Twist();
        double current_x=pose->x;
        double current_y=pose->y;
        RCLCPP_INFO(this->get_logger(),"当前位置(x=%f,y=%f)",current_x,current_y);
        double distance=std::sqrt((target_x_-current_x)*(target_x_-current_x)
                                +(target_y_-current_y)*(target_y_-current_y));
        double angle=std::atan2(target_y_-current_y,target_x_-current_x)-pose->theta;

        if(distance>0.1)
        {
            if(fabs(angle)>0.2) //绝对值
            {
                message.angular.z=fabs(angle);
            }
            else
            {
        message.linear.x=k_* distance;
            }
        }
        if(message.linear.x>max_speed_)
        {
            message.linear.x=max_speed_;
        }
        velocity_publisher_->publish(message);                        
    }
    rclcpp::Publisher<Twist>::SharedPtr velocity_publisher_;
    rclcpp::Subscription<Pose>::SharedPtr pose_subscription_;
    double target_x_=1.0;
    double target_y_=1.0;
    double k_=1.0;
    double max_speed_=3.0;
};
int main(int argc,char*argv[])
{
    rclcpp::init(argc,argv);
    rclcpp::spin(std::make_shared<TurtleControl>("turtle_controller_node_cpp"));
    rclcpp::shutdown();
    return 0;
}